"""
grid.py --                                                                                            
                                                                                                                                                                                                        
It sets the world's grid parameters.                                              
                                                                                                                                                                                                            
Date of creation: 2007-03-17                                                                        
                                                                                                                                                                                                
Copyright  Robotics and Automation Group, Pontificia Universidad Javeriana - Cali.                  
    Freddy Naranjo Perez, fnaranjo@puj.edu.co                                                                   
    Antonio Alejandro Matta Gomez amatta@puj.edu.co                                      
    Julian David Colorado, jdcolorado@puj.edu.co                           
    Juan Camilo Acosta Mejia, jcacosta@puj.edu.co                                    
                                                                                                                                                                                                        
See the file "license.terms" for information on usage and redistribution of this file, and for a    
DISCLAIMER OF ALL WARRANTIES.
"""

import vtk
from vtk.util.colors import *
import time
from graphengine import axes
from graphengine import grid
from graphengine import table
from graphengine import rail
from graphengine import box
from graphengine import base

class World:
    def __init__( self, ren ):
        """This class sets the 3D world structure."""
        self.type = 'none'
        self.timeSleep = 0.0025
    def InitWorldGrid ( self, ren ):
        """Inits Worlds grid."""
        
        #World's axes.
        self.worldaxes = axes.CreateAxes()
        
        #Grid's axes
        self.worldgrid = grid.Grid( ren )  
        ren.AddActor( self.worldaxes )
        
    def draw( self, ren, system):
        """It draws the world objects and the respective system."""
        
        #Main world assembly.
        self.Worldassembly = vtk.vtkAssembly()
        self.Worldassembly.SetPosition( 0.0, 0.0, 0.0 )
        self.Worldassembly.SetOrientation( 0.0, 0.0, 0.0 )
        (self.x,self.y,self.z)=ren.GetActiveCamera().GetPosition()
        self.focal = ren.GetActiveCamera().GetFocalPoint()
        
        #Create world according to system type.
        if (system.type =="cartpole"):
            #Create Cartpole's world objects.
            
            #Table: 
            #dimensions (length, height, width), pos. and orient. (xpos, ypos, zpos, r,p,y ).
            #Geometrical dimensions.
            tablegeomdim = [ system.TableLength, system.TableHeight, system.TableWidth, system.TableRadius ]
            #Position and orientation.
            tableposorient = [ -system.TableLength/2,  0.0, -system.TableWidth, 0.0, 0.0, 0.0 ]
            #Lighting properties: opacity, ambient, diffuse, specular, specularpower.
            tablelightprop = [ 0.4, 0.5, 0.6, 1.0, 10.0 ]
            #Color properties:ambient, diffuse, specular.
            tablecolorprop = [ ( 0.1, 0.1, 0.1 ), ( 0.1, 0.4, 0.8 ), ( 1.0, 1.0, 1.0 ) ]	
            self.table	=	table.Table( ren, tablegeomdim, tableposorient, tablelightprop, tablecolorprop )
            
            #Rail: 
            #dimensions (length, height, width).
            raildim = [  system.RailX, system.RailY, system.RailZ ]
            self.rail	= rail.Rail( ren, raildim )
            
            #Add objects into World assembly.
            self.Worldassembly.AddPart( self.table.tableassembly )
            self.Worldassembly.AddPart( self.rail.railassembly )
            
        elif ( system.type=="furuta" ):
            #Create Furuta's world objects.
            
            #Base
            #dimensions (length, radius), pos. and orient. (xpos, ypos, zpos, r,p,y ).
            #Geometrical dimensions.
            basegeomdim = [ 0.04 , 0.15 ]
            #Position and orientation.
            baseposorient = [ 0.0,  0.015, 0.0, 0.0, 0.0, 0.0 ]
            #Lighting properties: opacity, ambient, diffuse, specular, specularpower.
            baselightprop = [ 0.4, 0.5, 0.6, 1.0, 10.0 ]
            #Color properties:ambient, diffuse, specular.
            basecolorprop = [ ( 0.1, 0.1, 0.1 ), ( 0.1, 0.4, 0.8 ), ( 1.0, 1.0, 1.0 ) ]	
            self.base	=	base.Base( ren, basegeomdim, baseposorient, baselightprop, light_grey )
            
            #Add objects into World assembly.
            self.Worldassembly.AddPart( self.base.baseActor )
            
        elif (system.type =="pendubot"):
            #Create Pendubot's world objects.
            
            #Table: 
            #dimensions (length, height, width), pos. and orient. (xpos, ypos, zpos, r,p,y ), lighting, color.
            #Geometrical dimensions.
            geomdim = [ system.TableLength, system.TableHeight, system.TableWidth, system.TableRadius, system.TableRadius ]
            #Position and orientation.
            posorient = [ -system.TableLength/2,  0.0, -system.TableWidth+(system.CartZ/4), 0.0, 0.0, 0.0 ]
            #Lighting properties: opacity, ambient, diffuse, specular, specularpower.
            lightprop = [ 1.0, 0.5, 0.6, 1.0, 10.0 ]
            #Color properties:ambient, diffuse, specular.
            colorprop = [ ( 0.1, 0.1, 0.1 ), ( 0.2, 0.5, 0.1 ), ( 1.0, 1.0, 1.0 ) ]		
            self.table	=	table.Table( ren, geomdim, posorient, lightprop, colorprop )
            
            #Add objects into World assembly.
            self.Worldassembly.AddPart( self.table.tableassembly )
            
        elif ( system.type =="acrobot"):
            #Create Acrobot's world objects.
            
            #Table: 
            #dimensions (length, height, width), pos. and orient. (xpos, ypos, zpos, r,p,y ), lighting, color.
            #Geometrical dimensions.
            geomdim = [ system.TableLength, system.TableHeight, system.TableWidth, system.TableRadius ]
            #Position and orientation.
            posorient = [ -system.TableLength/2,  0.0, -system.TableWidth+(system.CartZ/4), 0.0, 0.0, 0.0 ]
            #Lighting properties: opacity, ambient, diffuse, specular, specularpower.
            lightprop = [ 1.0, 0.5, 0.6, 1.0, 10.0 ]
            #Color properties:ambient, diffuse, specular.
            colorprop = [ ( 0.1, 0.1, 0.1 ), ( 0.3, 0.5, 0.5 ), ( 1.0, 1.0, 1.0 ) ]		
            self.table	=	table.Table( ren, geomdim, posorient, lightprop, colorprop )
            
            #Add objects into World assembly.
            self.Worldassembly.AddPart( self.table.tableassembly )
            
        elif (system.type=="inertiawheel"):
            #Create Inertiawheel's world objects.
            
            #Table: 
            #dimensions (length, height, width), pos. and orient. (xpos, ypos, zpos, r,p,y ), lighting, color.
            #Geometrical dimensions.
            geomdim = [ system.TableLength, system.TableHeight, system.TableWidth, system.TableRadius ]
            #Position and orientation.
            posorient = [ -system.TableLength/2,  0.0, -system.TableWidth+system.CartZ/6, 0.0, 0.0, 0.0 ]
            #Lighting properties: opacity, ambient, diffuse, specular, specularpower.
            lightprop = [ 1.0, 0.5, 0.6, 1.0, 10.0 ]
            #Color properties:ambient, diffuse, specular.
            colorprop = [ ( 0.1, 0.1, 0.1 ), ( 0.2, 0.5, 0.1 ), ( 1.0, 1.0, 1.0 ) ]		
            self.table	=	table.Table( ren, geomdim, posorient, lightprop, colorprop )
            
            #Add objects into World assembly.
            self.Worldassembly.AddPart( self.table.tableassembly )
        
        #Add system assembly into world assembly
        system.draw( ren )
        self.Worldassembly.AddPart( system.Systemassembly )
        #Render World assembly.
        ren.AddActor( self.Worldassembly )
        
    def fadein( self, ren, VTKwindow, system):
        """This function controls the system's showing"""
        
        i = -0.8
        while ( i <=0.0 ):
            time.sleep( self.timeSleep )
            i = i+0.01
            self.Worldassembly.SetPosition( 0.0, 0.0, i)
            VTKwindow.Render()
        
        #This is for positioning the camera according to system type.
        if (system.type =="pendubot" or system.type =="acrobot"):
            #camera's zoom.
            (a,b,c)=ren.GetActiveCamera().GetPosition()
            i = 0
            while ( i <=0.9 ):
                #time.sleep( 0.025 )
                i = i+0.01
                ren.GetActiveCamera().SetPosition( a, b, c-i)
                VTKwindow.Render()
                
        if (system.type =="inertiawheel" or system.type =="furuta"):
            #camera's pitch
            i = 0
            while ( i <=0.25 ):
                time.sleep( self.timeSleep )
                i = i+0.01
                ren.GetActiveCamera().Pitch( -i )
                VTKwindow.Render()
                
            #camera's elevation
            i = 0
            while ( i <=0.4 ):
                time.sleep( self.timeSleep )
                i = i+0.01
                ren.GetActiveCamera().Elevation( -i )
                VTKwindow.Render()
                
            #camera's zoom.
            (a,b,c)=ren.GetActiveCamera().GetPosition()
            i = 0
            if (system.type =="inertiawheel"):
                imax = 1.7
            else:
                imax = 1.3
            while ( i <=imax ):
                time.sleep( self.timeSleep )
                i = i+0.02
                ren.GetActiveCamera().SetPosition( a, b, c-i)
                VTKwindow.Render()
                
        
                
    def fadeout( self, ren, VTKwindow, system ):
        """This function controls the system's dissapearing"""
        i = 0.0
        while ( i >(-0.8) ):
            time.sleep( self.timeSleep )
            i = i-0.01
            self.Worldassembly.SetPosition( 0.0, 0.0, i)
            VTKwindow.Render()
        self.Worldassembly.SetVisibility(0)
        
        ren.GetActiveCamera().SetPosition( self.x, self.y, self.z)
        ren.GetActiveCamera().SetFocalPoint( self.focal)
        
    def DeleteWorldGrid ( self ):
        """Deletes Worlds grid."""
        del self.worldgrid.planeActor
    
    
    
  
        
    
    

        
        
            
        
    

    
  
  